#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static void _warp_perspective(const cv::Mat& src, const cv::Mat& M, cv::Mat& dst) {
    dst.create(src.size(), src.type());
    cv::Mat MM;
    cv::invert(M, MM);
    
    double m0 = MM.ptr<double>(0)[0];
    double m1 = MM.ptr<double>(0)[1];
    double m2 = MM.ptr<double>(0)[2];
    double m3 = MM.ptr<double>(1)[0];
    double m4 = MM.ptr<double>(1)[1];
    double m5 = MM.ptr<double>(1)[2];
    double m6 = MM.ptr<double>(2)[0];
    double m7 = MM.ptr<double>(2)[1];
    double m8 = MM.ptr<double>(2)[2];
    for (int y = 0; y < dst.rows; y++) {
        for (int x = 0; x < dst.cols; x++) {
            double xp = m0 * x + m1 * y + m2;
            double yp = m3 * x + m4 * y + m5;
            double w = m6 * x + m7 * y + m8;
            w = w ? 1. / w : 0;
            xp *= w;
            yp *= w;
            double fx = std::max((double)INT_MIN, std::min((double)INT_MAX, xp));
            double fy = std::max((double)INT_MIN, std::min((double)INT_MAX, yp));
            int X = cv::saturate_cast<int>(fx);
            int Y = cv::saturate_cast<int>(fy);
            short src_x = cv::saturate_cast<short>(X);
            short src_y = cv::saturate_cast<short>(Y);
            if (src_x >= 0 && src_x < src.cols && src_y >= 0 && src_y < src.rows) {
                dst.ptr<uchar>(y)[x] = src.ptr<uchar>(src_y)[src_x];
            } else {
                dst.ptr<uchar>(y)[x] = 0;
            }
        }
    }
}

static cv::Mat _get_perspective_matrix(const cv::Point2f* src, const cv::Point2f* dst, int solveMethod) {
    cv::Mat M;
    M.create(9, 1, CV_64FC1);
    
    cv::Mat A, X, b;
    A.create(8, 8, CV_64FC1);
    X.create(8, 1, CV_64FC1);
    b.create(8, 1, CV_64FC1);
    
    for (int i = 0; i < 4; i++) {
        double x = src[i].x;
        double y = src[i].y;
        double xp = dst[i].x;
        double yp = dst[i].y;
        
        A.ptr<double>(i)[0] = A.ptr<double>(i+4)[3] = x;
        A.ptr<double>(i)[1] = A.ptr<double>(i+4)[4] = y;
        A.ptr<double>(i)[2] = A.ptr<double>(i+4)[5] = 1;
        A.ptr<double>(i)[3] = A.ptr<double>(i+4)[0] = 
        A.ptr<double>(i)[4] = A.ptr<double>(i+4)[1] = 
        A.ptr<double>(i)[5] = A.ptr<double>(i+4)[2] = 0;
        A.ptr<double>(i)[6] = -x * xp;
        A.ptr<double>(i)[7] = -y * xp;
        A.ptr<double>(i+4)[6] = -x * yp;
        A.ptr<double>(i+4)[7] = -y * yp;
        
        b.ptr<double>(i)[0] = xp;
        b.ptr<double>(i+4)[0] = yp;
    }
    
    cv::solve(A, b, X, solveMethod);
    X.copyTo(cv::Mat(M, cv::Rect(0, 0, 1, 8)));
    M.ptr<double>(8)[0] = 1.0;
    M = M.reshape(0, 3);
    
    return M;
}

cv::Mat src, mat, dst;
int pts_count = 0;
cv::Point2f pts[4];

static void _mouse_double_click_handler(int nEvt, int x, int y, int flags, void* p) {
    switch (nEvt) {
        case cv::EVENT_LBUTTONDBLCLK:
            if (pts_count < 4) {
                pts[pts_count] = cv::Point2f(x, y);
                pts_count++;
                cv::circle(mat, cv::Point(x, y), 5, cv::Scalar(255, 0, 0), -1, 8);
                cv::imshow("src", mat);
            }
            break;
        default:
            break;
    }
    if (pts_count == 4) {
        cv::Point2f pt2[4];
        pt2[0] = cv::Point2f(0, 0);
        pt2[1] = cv::Point2f(400, 0);
        pt2[2] = cv::Point2f(400, 400);
        pt2[3] = cv::Point2f(0, 400);
        cv::Mat M = cv::getPerspectiveTransform(pts, pt2);
        cv::warpPerspective(src, dst, M, cv::Size(400, 400));
        cv::imshow("dst", dst);
    }
}

int main(int argc, char **argv) {
    //cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    src = cv::imread("/home/xlll/Downloads/opencv/samples/data/sudoku.png", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "Failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    
    /*
    cv::Point2f pt1[4], pt2[4];
    float xoffset = src.cols / 5;
    float yoffset = src.rows / 3;
    pt1[0] = cv::Point2f(0, 0);
    pt1[1] = cv::Point2f(xoffset, 0);
    pt1[2] = cv::Point2f(xoffset, yoffset);
    pt1[3] = cv::Point2f(0, yoffset);
    pt2[0] = cv::Point2f(xoffset, yoffset);
    pt2[1] = cv::Point2f(xoffset*2, yoffset);
    pt2[2] = cv::Point2f(xoffset*2, yoffset*2);
    pt2[3] = cv::Point2f(xoffset, yoffset*2);
    cv::Mat M, M1;
    M = cv::getPerspectiveTransform(pt1, pt2);
    M1 = cv::getAffineTransform(pt1, pt2);
    
    cv::Point2f center(0, 0);
    double angle = 30;
    double angle_radian = 30 * CV_PI / 180;
    double scale = 1;
    M1 = cv::getRotationMatrix2D(center, angle, scale);
    M1.copyTo(cv::Mat(M, cv::Rect(0, 0, 3, 2)));
    M.ptr<double>(2)[0] = 0;
    M.ptr<double>(2)[1] = 0;
    M.ptr<double>(2)[2] = 1;
    
    double scalex = 0.5;
    double scaley = 2;
    pt1[0] = cv::Point2f(0, 0);
    pt1[1] = cv::Point2f(xoffset, 0);
    pt1[2] = cv::Point2f(xoffset, yoffset);
    pt1[3] = cv::Point2f(0, yoffset);
    pt2[0] = cv::Point2f(0, 0);
    pt2[1] = cv::Point2f(xoffset*scalex, 0);
    pt2[2] = cv::Point2f(xoffset*scalex, yoffset*scaley);
    pt2[3] = cv::Point2f(0, yoffset*scaley);
    M = cv::getPerspectiveTransform(pt1, pt2);
    M1 = cv::getAffineTransform(pt1, pt2);
    
    pt1[0] = cv::Point2f(0, 0);
    pt1[1] = cv::Point2f(xoffset, 0);
    pt1[2] = cv::Point2f(xoffset, yoffset);
    pt1[3] = cv::Point2f(0, yoffset);
    pt2[0] = cv::Point2f(0, 0);
    pt2[1] = cv::Point2f(xoffset, 0);
    pt2[2] = cv::Point2f(yoffset*std::tan(angle_radian) + xoffset, yoffset);
    pt2[3] = cv::Point2f(yoffset*std::tan(angle_radian), yoffset);
    M = cv::getPerspectiveTransform(pt1, pt2);
    M1 = cv::getAffineTransform(pt1, pt2);
    
    pt2[1] = cv::Point2f(xoffset, xoffset*std::tan(angle_radian));
    pt2[2] = cv::Point2f(xoffset, xoffset*std::tan(angle_radian)+yoffset);
    pt2[3] = cv::Point2f(0, yoffset);
    M = cv::getPerspectiveTransform(pt1, pt2);
    M1 = cv::getAffineTransform(pt1, pt2);
    
    //std::cout << "M = " << M << std::endl;
    //std::cout << "M1 = " << M1 << std::endl;
    
    for (int i = 0; i < 4; i++) {
        pt1[i] = cv::Point2f(std::rand() % src.cols, std::rand() % src.rows);
        pt2[i] = cv::Point2f(std::rand() % src.cols, std::rand() % src.rows);
    }
    
    int solveMethod = cv::DECOMP_SVD;
    M = cv::getPerspectiveTransform(pt1, pt2, solveMethod);
    M1 = _get_perspective_matrix(pt1, pt2, solveMethod);
    std::cout << "M = " << std::endl << M << std::endl;
    std::cout << "M1 = " << std::endl << M1 << std::endl;
    
    cv::Mat dst;
    cv::warpPerspective(src, dst, M, src.size(), cv::INTER_NEAREST);
    cv::Mat dst2;
    _warp_perspective(src, M, dst2);
    
    std::vector<cv::Point> non_zeros;
    cv::Mat nonzeroMat = dst != dst2;
    cv::findNonZero(nonzeroMat, non_zeros);
    std::cout << "non zero size = " << non_zeros.size() << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("dst", dst);
    cv::waitKey(0);
    */
    
    cv::namedWindow("src");
    cv::setMouseCallback("src", _mouse_double_click_handler);
    mat = src.clone();
    cv::imshow("src", mat);
    cv::waitKey(0);
    
    return 0;
}
